#include "VSIKController.h"
#include "VSGraphicInclude.h"
#include "VSStream.h"
using namespace VSEngine2;
IMPLEMENT_RTTI(VSIKController,VSAnimBaseFunction)
static bool gs_bStreamRegistered_VSIKController= VSIKController::RegisterMainFactory();
bool VSIKController::ms_bRegisterMainFactory = false;
bool VSIKController::RegisterMainFactory()
{
	if (!ms_bRegisterMainFactory)
	{ 
		VSMain::AddInitialFuntion(VSIKController::InitialClassFactory); 
		ms_bRegisterMainFactory = true;
	} 
	return ms_bRegisterMainFactory;

}
bool VSIKController::InitialClassFactory()
{
	ms_ClassFactory.AddElement(ms_Type.GetName(),FactoryFunc);

	return 1;
}
VSObject * VSIKController::FactoryFunc()
{
	return VS_NEW VSIKController;

}

bool VSIKController::Register(VSStream & rStream)const
{
	if (rStream.IsObjectRegister(this))
	{
		return 1;
	}
	if(!VSAnimBaseFunction::Register(rStream))
		return 0;
	for (unsigned int i = 0 ; i < m_pIKNode.GetNum() ; i++)
	{
		if(m_pIKNode[i])
			m_pIKNode[i]->Register(rStream);
	}
	return 1;
}
unsigned int VSIKController::DiskUse()const
{
	unsigned int Used = VSAnimBaseFunction::DiskUse();

	Used += VSStream::GetStrDistUse(m_RootBoneName);

	Used += sizeof(unsigned int);

	Used += sizeof(VSBoneNode *) * m_pIKNode.GetNum();

	Used += sizeof(unsigned int);

	Used += sizeof(VSBoneNode *) * m_pEffector.GetNum();

	Used += sizeof(unsigned int);

	Used += sizeof(VSREAL);

	Used += sizeof(bool) * 3;
	return Used;
}
bool VSIKController::Save(VSStream & rStream,unsigned int &iSaveUse)const
{
	if(!VSAnimBaseFunction::Save(rStream,iSaveUse))
		return 0;

	if(!rStream.WriteString(m_RootBoneName))
		return 0;
	iSaveUse = VSStream::GetStrDistUse(m_RootBoneName);

	unsigned int uiIKNodeNum = m_pIKNode.GetNum();
	if(!rStream.Write(&uiIKNodeNum,sizeof(unsigned int)))
		return 0;
	iSaveUse += sizeof(unsigned int);

	for (unsigned int i = 0 ; i < m_pIKNode.GetNum() ; i++)
	{
// 		VSBoneNode * pBoneNode = m_pIKNode[i];
// 		if(!rStream.Write(&pBoneNode,sizeof(VSBoneNode *)))
// 			return 0;
		if(!rStream.WriteObjectPtr(m_pIKNode[i]))
			return 0;
		iSaveUse += sizeof(VSBoneNode *);
	}
	

	unsigned int uiEffectorNum = m_pEffector.GetNum();
	if(!rStream.Write(&uiEffectorNum,sizeof(unsigned int)))
		return 0;
	iSaveUse += sizeof(unsigned int);

	for (unsigned int i = 0 ; i < m_pEffector.GetNum() ; i++)
	{
// 		VSBoneNode * pBoneNode = m_pEffector[i];
// 		if(!rStream.Write(&pBoneNode,sizeof(VSBoneNode *)))
// 			return 0;
		if(!rStream.WriteObjectPtr(m_pEffector[i]))
			return 0;
		iSaveUse += sizeof(VSBoneNode *);
	}

	if(!rStream.Write(&m_uiIterations,sizeof(unsigned int)))
		return 0;
	iSaveUse += sizeof(unsigned int);

	if(!rStream.Write(&m_fTargetThreshold,sizeof(VSREAL)))
		return 0;
	iSaveUse += sizeof(VSREAL);

	if(!rStream.Write(&m_bOrderRToE,sizeof(bool)))
		return 0;
	iSaveUse += sizeof(bool);

	if(!rStream.Write(&m_bAllowTranslation,sizeof(bool)))
		return 0;
	iSaveUse += sizeof(bool);

	if(!rStream.Write(&m_bAllowRotation,sizeof(bool)))
		return 0;
	iSaveUse += sizeof(bool);

	return 1;
}
bool VSIKController::Load(VSStream & rStream,unsigned int &iSaveUse)
{	
	if(!VSAnimBaseFunction::Load(rStream,iSaveUse))
		return 0;
	if(!rStream.ReadString(m_RootBoneName))
		return 0;
	iSaveUse = VSStream::GetStrDistUse(m_RootBoneName);

	unsigned int uiIKNodeNum = 0;
	if(!rStream.Read(&uiIKNodeNum,sizeof(unsigned int)))
		return 0;
	iSaveUse += sizeof(unsigned int);

	m_pIKNode.SetBufferNum(uiIKNodeNum);

	for (unsigned int i = 0 ; i < m_pIKNode.GetNum() ; i++)
	{
// 		VSBoneNode * pBoneNode = NULL;
// 		if(!rStream.Write(&pBoneNode,sizeof(VSBoneNode *)))
// 			return 0;
// 		AddLink(pBoneNode);
		if(!rStream.ReadObjectPtr(m_pIKNode[i]))
			return 0;
		iSaveUse += sizeof(VSBoneNode *);
	}


	unsigned int uiEffectorNum = 0;
	if(!rStream.Read(&uiEffectorNum,sizeof(unsigned int)))
		return 0;
	iSaveUse += sizeof(unsigned int);

	m_pEffector.SetBufferNum(uiEffectorNum);

	for (unsigned int i = 0 ; i < m_pEffector.GetNum() ; i++)
	{
// 		VSBoneNode * pBoneNode = NULL;
// 		if(!rStream.Write(&pBoneNode,sizeof(VSBoneNode *)))
// 			return 0;
// 		AddLink(pBoneNode);
		if(!rStream.ReadObjectPtr(m_pEffector[i]))
			return 0;
		iSaveUse += sizeof(VSBoneNode *);
	}


	if(!rStream.Read(&m_uiIterations,sizeof(unsigned int)))
		return 0;
	iSaveUse += sizeof(unsigned int);

	if(!rStream.Read(&m_fTargetThreshold,sizeof(VSREAL)))
		return 0;
	iSaveUse += sizeof(VSREAL);

	if(!rStream.Read(&m_bOrderRToE,sizeof(bool)))
		return 0;
	iSaveUse += sizeof(bool);

	if(!rStream.Read(&m_bAllowTranslation,sizeof(bool)))
		return 0;
	iSaveUse += sizeof(bool);

	if(!rStream.Read(&m_bAllowRotation,sizeof(bool)))
		return 0;
	iSaveUse += sizeof(bool);
	return 1;
}
bool VSIKController::Link(VSStream & rStream)
{
	if(!VSAnimBaseFunction::Link(rStream))
		return 0;
	for (unsigned int i = 0 ; i < m_pIKNode.GetNum() ; i++)
	{
		//m_pIKNode[i] = (VSBoneNode *)rStream.GetMapValue(GetNextLink());
		rStream.LinkObjectPtr(m_pIKNode[i]);
	}

	for (unsigned int i = 0 ; i < m_pIKNode.GetNum() ; i++)
	{
		/*m_pEffector[i] = (VSBoneNode *)rStream.GetMapValue(GetNextLink());*/
		rStream.LinkObjectPtr(m_pEffector[i]);
	}
	return 1;

}
bool VSIKController::Clone(const VSObject *pObject,unsigned int uiType)
{
	const VSIKController* Temp = DynamicCast<VSIKController>(pObject); 
	if(!Temp)
		return 0;
	if(VSAnimBaseFunction::Clone(pObject,uiType))
	{
		m_RootBoneName = Temp->m_RootBoneName;
		m_uiIterations = Temp->m_uiIterations;
		m_fTargetThreshold = Temp->m_fTargetThreshold;

		m_bOrderRToE = Temp->m_bOrderRToE;
		m_bAllowTranslation = Temp->m_bAllowTranslation;
		m_bAllowRotation = Temp->m_bAllowRotation;
		return 1;

	}
	else
	{
		return 0;
	}

}
VSIKController::~VSIKController()
{

}
VSIKController::VSIKController(const VSUsedName & ShowName,VSAnimTree * pAnimTree)
				:VSAnimBaseFunction(ShowName,pAnimTree)
{
	m_fTargetThreshold = 1.0f;
	m_uiIterations = 10;

	m_bOrderRToE = true;
	m_bAllowTranslation = false;
	m_bAllowRotation = true;

	VSString OutputName = _T("Output");
	VSOutputNode * pOutputNode = NULL;
	pOutputNode = VS_NEW VSOutputNode(VSPutNode::AVT_IK,OutputName,this);
	VSMAC_ASSERT(pOutputNode);
	m_pOutput.AddElement(pOutputNode);

}
VSIKController::VSIKController()
{
	m_fTargetThreshold = 1.0f;
	m_uiIterations = 10;
	m_bOrderRToE = true;
	m_bAllowTranslation = false;
	m_bAllowRotation = true;

}
void VSIKController::SetRootBoneName(const VSString & RootBoneName)
{
	m_RootBoneName = RootBoneName;
	m_pObject = m_pOwner->GetSkelectonMeshNode()->GetSkelecton()->GetBoneNode(m_RootBoneName);
	if (!m_pObject)
	{
		return ;
	}
	m_pIKNode.Clear();
	m_pEffector.Clear();
	VSArray<VSBoneNode *> m_pIKNodeTemp;

	((VSBoneNode *)m_pObject)->GetAllBoneArray(m_pIKNodeTemp);
	for (unsigned int  i =0 ; i < m_pIKNodeTemp.GetNum() ; i++)
	{
		if (m_pIKNodeTemp[i])
		{
			if(((VSBoneNode *)m_pIKNodeTemp[i])->m_bIsEffector)
			{
				m_pEffector.AddElement((VSBoneNode *)m_pIKNodeTemp[i]);
			}
			m_pIKNode.AddElement((VSBoneNode *)m_pIKNodeTemp[i]);
		}
	}
}
bool VSIKController::UpdateEx(double dAppTime)
{
	if (!m_pObject)
	{
		m_pObject = m_pOwner->GetSkelectonMeshNode()->GetSkelecton()->GetBoneNode(m_RootBoneName);
		if (!m_pObject)
		{
			return 1;
		}
		m_pIKNode.Clear();
		m_pEffector.Clear();
		VSArray<VSBoneNode *> m_pIKNodeTemp;

		((VSBoneNode *)m_pObject)->GetAllBoneArray(m_pIKNodeTemp);
		for (unsigned int  i =0 ; i < m_pIKNodeTemp.GetNum() ; i++)
		{
			if (m_pIKNodeTemp[i])
			{
				if(((VSBoneNode *)m_pIKNodeTemp[i])->m_bIsEffector)
				{
					m_pEffector.AddElement((VSBoneNode *)m_pIKNodeTemp[i]);
				}
				m_pIKNode.AddElement((VSBoneNode *)m_pIKNodeTemp[i]);
			}
		}
		return 1;
	}
	if(!m_pIKNode.GetNum())
		return 1;

	unsigned int uiCurIterations = 0;
	int CurNode = 0;

	if (!m_bOrderRToE)
	{
		CurNode = m_pIKNode.GetNum() - 1;
	}

	while(GetCurError() > m_fTargetThreshold && uiCurIterations < m_uiIterations)
	{
		uiCurIterations++;
		if (CurNode >= (int)m_pIKNode.GetNum())
		{
			CurNode = 0;
		}
		if (CurNode < 0)
		{
			CurNode = m_pIKNode.GetNum() - 1;
		}
		UpdateLocalTranslation(m_pIKNode[CurNode],dAppTime);
		UpdateLocalRotation(m_pIKNode[CurNode],dAppTime);
		if (!m_bOrderRToE)
		{
			CurNode--;
		}
		else
			CurNode++;
	}
	
	return 1;
}
VSREAL VSIKController::GetCurError()const
{
	VSREAL fError = 0.0f;
	for(unsigned int i = 0 ; i < m_pEffector.GetNum() ; i++)
	{
		VSBoneNode * pBoneNode =  m_pEffector[i];
		if (pBoneNode)
		{

			VSVector3 Diff =  pBoneNode->GetWorldTranslate() - pBoneNode->m_TargetPosInWorld;

			fError += Diff.GetSqrLength();

		}

	}
	return fError;
}
void VSIKController::UpdateLocalTranslation(VSBoneNode * pIKBone,double dAppTime)
{
	if (!pIKBone)
	{
		return;
	}
	bool bChanged = false;
	VSREAL fTotleWeight = 0.0f;
	
	for (unsigned int j = 0 ; j < m_pEffector.GetNum() ; j++)
	{
		VSBoneNode * pIKBoneNode = m_pEffector[j];
		
		fTotleWeight += pIKBoneNode->m_fIKWeight;
	}

	if (fTotleWeight < EPSILON_E4)
	{
		return;
	}

	VSVector3 Axis[3];
	pIKBone->GetIKMoveAxis(Axis);
	VSVector3 LocalTran = pIKBone->GetLocalTranslate();

	
	for(unsigned int i = 0 ; i < 3 ; i++)
	{
		if (!pIKBone->m_bAllowTranslation[i])
		{
			continue;
		}

		
		VSREAL fNumber = 0.0f;
		VSREAL fOldDiff = 0;
		
		for (unsigned int j = 0 ; j < m_pEffector.GetNum() ; j++)
		{		
			VSBoneNode * pIKBoneNode = m_pEffector[j];
			VSVector3 DiffTemp = pIKBoneNode->m_TargetPosInWorld - pIKBoneNode->GetWorldTranslate();
			fOldDiff += DiffTemp.GetSqrLength();
			fNumber += pIKBoneNode->m_fIKWeight * (Axis[i] * DiffTemp);
		}

		if (fNumber < EPSILON_E4)
		{
			continue;
		}
		VSREAL fT = fNumber / fTotleWeight;

		VSREAL fTrani = LocalTran.m[i];
		VSREAL fDesired = fTrani + fT;
		if (fDesired > pIKBone->m_fMinTranslation[i])
		{
			if (fDesired < pIKBone->m_fMaxTranslation[i])
			{
				fTrani = fDesired;
			}
			else
			{
				fT = pIKBone->m_fMaxTranslation[i] - fTrani;
				fTrani = pIKBone->m_fMaxTranslation[i];
			}
		}
		else
		{
			fT = pIKBone->m_fMinTranslation[i] - fTrani;
			fTrani = pIKBone->m_fMinTranslation[i];
		}

		VSVector3 Delta = Axis[i] * fT;

		VSREAL fNewDiff = 0;
		for (unsigned int j = 0 ; j < m_pEffector.GetNum() ; j++)
		{		
			VSBoneNode * pIKBoneNode = m_pEffector[j];
			VSVector3 DiffTemp = pIKBoneNode->m_TargetPosInWorld - (pIKBoneNode->GetWorldTranslate() + Delta);
			fNewDiff += DiffTemp.GetSqrLength();
		}

		if (fNewDiff >= fOldDiff)
		{
			continue;
		}
		LocalTran.m[i] = fTrani;
		bChanged = true;
	}

	if (bChanged = true)
	{
		pIKBone->SetLocalTranslate(LocalTran);
		pIKBone->UpdateAll(dAppTime);
	}
	
}
void VSIKController::UpdateLocalRotation(VSBoneNode * pIKBone,double dAppTime)
{
	if (!pIKBone)
	{
		return;
	}

	VSVector3 Axis[3];
	Axis[0] = VSVector3(1.0f,0.0f,0.0f);
	Axis[1] = VSVector3(0.0f,1.0f,0.0f);
	Axis[2] = VSVector3(0.0f,0.0f,1.0f);
	VSArray<VSVector3> ELP;
	VSArray<VSVector3> GLP;
	ELP.SetBufferNum(m_pEffector.GetNum());
	GLP.SetBufferNum(m_pEffector.GetNum());

	

	for(unsigned int i = 0 ; i < 3 ; i++)
	{
		if (!pIKBone->m_bAllowRotation[i])
		{
			continue;
		}
		VSVector3 Angle;
		pIKBone->GetLocalRotate().GetEluer(Angle.x,Angle.y,Angle.z);

		VSREAL fNumber1 = 0.0f;
		VSREAL fNumber2 = 0.0f;
		VSREAL fOldDiff = 0;

		for (unsigned int j = 0 ; j < m_pEffector.GetNum() ; j++)
		{
			VSBoneNode * pIKBoneNode = m_pEffector[j];
			
			if (pIKBone->GetParent())
			{
				VSTransform Tran = pIKBone->GetParent()->GetWorldTransform();
				ELP[j] = pIKBoneNode->GetWorldTranslate() * Tran.GetCombineInverse();
				GLP[j] = pIKBoneNode->m_TargetPosInWorld * Tran.GetCombineInverse();
			}
			else
			{
				ELP[j] = pIKBoneNode->GetWorldTranslate();
				GLP[j] = pIKBoneNode->m_TargetPosInWorld;
			}
			VSVector3 EToP = ELP[j] - pIKBone->GetLocalTranslate();

			VSVector3 GToP = GLP[j] - pIKBone->GetLocalTranslate();

			VSVector3 GToE = GLP[j] - ELP[j];
			
			fOldDiff += GToE.GetSqrLength();

			VSVector3 UxEToP;
			UxEToP.Cross(Axis[i],EToP);

			VSVector3 UxUxETop;
			UxUxETop.Cross(Axis[i],UxEToP);

			fNumber1 += pIKBoneNode->m_fIKWeight * (GToP * UxEToP);
			fNumber2 -= pIKBoneNode->m_fIKWeight * (GToP * UxUxETop);

		}
		if (fNumber1 * fNumber1 + fNumber2 * fNumber2 <= EPSILON_E4)
		{
			continue;
		}
		VSREAL fT = ATAN2(fNumber1,fNumber2);
		if (ABS(fT) < EPSILON_E4)
		{
			continue;
		}
		VSREAL fAngle = Angle.m[i];
		VSREAL fDesired = fAngle + fT;

		if (fDesired > pIKBone->m_fMinRotation[i])
		{
			if (fDesired < pIKBone->m_fMaxRotation[i])
			{
				fAngle = fDesired;
			}
			else
			{
				fT = pIKBone->m_fMaxRotation[i] - fAngle;
				fAngle = pIKBone->m_fMaxRotation[i];
			}
		}
		else
		{
			fT = pIKBone->m_fMinRotation[i] - fAngle;
			fAngle = pIKBone->m_fMinRotation[i];
		}

		VSREAL fNewDiff = 0;

		VSMatrix3X3 Mat;
		Mat.CreateAxisAngle(Axis[i],fT);

		for (unsigned int j = 0 ; j < m_pEffector.GetNum() ; j++)
		{		
			VSBoneNode * pIKBoneNode = m_pEffector[j];

			VSVector3 EToP = ELP[j] - pIKBone->GetLocalTranslate();

			VSVector3 NewE = pIKBone->GetLocalTranslate() + EToP * Mat;

			VSVector3 DiffTemp = GLP[j] - NewE;

			fNewDiff += DiffTemp.GetSqrLength();
		}

		if (fNewDiff >= fOldDiff)
		{
			continue;
		}
		Angle.m[i] = fAngle;

		VSMatrix3X3 MatNew;
		MatNew.CreateRotXYZ(Angle);
		pIKBone->SetLocalRotate(MatNew);
		pIKBone->UpdateAll(dAppTime);
	}

		

}
// void VSIKController::UpdateLocalRotation(VSBoneNode * pIKBone,double dAppTime)
// {
// 	if (!pIKBone)
// 	{
// 		return;
// 	}
// 	bool bChanged = false;
// 
// 	VSVector3 Axis[3];
// 	pIKBone->GetIKMoveAxis(Axis);
// 
// 	
// 
// 	
// 	for(unsigned int i = 0 ; i < 3 ; i++)
// 	{
// 		if (!pIKBone->m_bAllowRotation[i])
// 		{
// 			continue;
// 		}
// 
// 		VSREAL fNumber1 = 0.0f;
// 		VSREAL fNumber2 = 0.0f;
// 		VSREAL fOldDiff = 0;
// 
// 		VSVector3 Angle;
// 		pIKBone->GetLocalRotate().GetEluer(Angle.x,Angle.y,Angle.z);
// 		for (unsigned int j = 0 ; j < m_pEffector.GetNum() ; j++)
// 		{
// 			VSBoneNode * pIKBoneNode = m_pEffector[j];
// 
// 			VSVector3 EToP = pIKBoneNode->GetWorldTranslate() - pIKBone->GetWorldTranslate();
// 
// 			VSVector3 GToP = pIKBoneNode->m_TargetPosInWorld - pIKBone->GetWorldTranslate();
// 
// 			VSVector3 GToE = pIKBoneNode->m_TargetPosInWorld - pIKBoneNode->GetWorldTranslate();
// 
// 			fOldDiff += GToE.GetSqrLength();
// 
// 			VSVector3 UxEToP;
// 			UxEToP.Cross(Axis[i],EToP);
// 
// 			VSVector3 UxUxETop;
// 			UxUxETop.Cross(Axis[i],UxEToP);
// 
// 			fNumber1 += pIKBoneNode->m_fIKWeight * (GToP * UxEToP);
// 			fNumber2 -= pIKBoneNode->m_fIKWeight * (GToP * UxUxETop);
// 
// 		}
// 		if (fNumber1 * fNumber1 + fNumber2 * fNumber2 <= EPSILON_E4)
// 		{
// 			continue;
// 		}
// 		VSREAL fT = 0;
// 		fT = ATAN2(fNumber1,fNumber2);
// 		if (ABS(fT) < EPSILON_E4)
// 		{
// 			continue;
// 		}
// 		VSMatrix3X3 Comb;
// 		
// 		Comb.CreateAxisAngle(Axis[i],fT);
// 		
// 
// 		VSMatrix3X3 LocComb;
// 		pIKBone->ComputeIKLocalRotDelta(Comb,LocComb);
// 
// 		VSVector3 AngleDelta;
// 
// 		LocComb.GetEluer(AngleDelta.x,AngleDelta.y,AngleDelta.z);
// 
// 		for(unsigned int i = 0 ; i < 3 ; i++)
// 		{
// 			VSREAL &fAngle = Angle.m[i];
// 			VSREAL fDesired = fAngle + AngleDelta.m[i];
// 
// 			if (fDesired > pIKBone->m_fMinRotation[i])
// 			{
// 				if (fDesired < pIKBone->m_fMaxRotation[i])
// 				{
// 					fAngle = fDesired;
// 				}
// 				else
// 				{
// 					fAngle = pIKBone->m_fMaxRotation[i];
// 				}
// 			}
// 			else
// 			{
// 				fAngle = pIKBone->m_fMinRotation[i];
// 			}
// 
// 		}
// 
// 
// 		VSMatrix3X3 MatNew;
// 		MatNew.CreateRotXYZ(Angle);
// 		pIKBone->SetLocalRotate(MatNew);
// 		pIKBone->UpdateAll(dAppTime);
// 	}
// 
// }